五杆机构正运动学公式、逆运动学公式MATLAB代码 |
您所在的位置：网站首页 › matlab 动力学 › 五杆机构正运动学公式、逆运动学公式MATLAB代码 |
五杆机构正运动学公式、逆运动学公式MATLAB代码
|
在研究五杆机构的时候感觉网上的资料更多的在研究正运动学，包括机构顶端的运动轨迹、速度、加速度，缺少逆运动学的分析。当我们想用五杆机构做实际用途比如3D打印或轨迹描绘的时候逆运动学公式是必不可少的。所以我觉得写一篇汇总正逆运动学的文章还是有必要的。 一、正运动学： 正运动学公式推导主要参考这篇文章：平面五杆机构运动学和动力学特性分析 MATLAB正运动学代码： function [xc,yc] = F(u1,u4,l1,l2,l3,l4,l5) xb = l1 * cos(u1); yb = l1 * sin(u1); xd = l5 + l4 * cos(u4); yd = l4 * sin(u4); lbd = sqrt((xd - xb).^2 + (yd - yb).^2); A0 = 2 * l2 * (xd - xb); B0 = 2 * l2 * (yd - yb); C0 = l2.^2 + lbd.^2 - l3.^2; u2 = 2 * atan((B0 + sqrt(A0.^2 + B0.^2 - C0.^2))/(A0 + C0)); xc = xb + l2 * cos(u2); yc = yb + l2 * sin(u2); end其中l1、l2、l3、l4、l5长度分别对应图1中的相应标号的杆子的长度。u1、u4就是φ1、φ4单位°，正运动学根据给定的7个参数求出C点相对于坐标原点（A点）的X\Y信息。 二、逆运动学公式 逆运动学主要参考David Tavkhelidze的PPT，由于文章被墙，这里给个百度云链接，或者博客下载地址。 提取码: 6hyu 主要逆运动学公式在PPT里面的公式14、15、17、18. MATLAB逆运动学代码： function [thta1,thta2] = inverseF(x,y,l1,l2,l3,l4,l0) Xc = x; %Xc∈（35,80） Yc = y; %Yc∈（75,115） cosfoai_12 = (Xc.^2 + Yc.^2 - l1.^2 - l2.^2)/(2 * l1 * l2); %foai_12∈（pi，2pi） foai_12 = 2*pi - acos(cosfoai_12); cosfoai_01 = (l2*Yc*sin(foai_12)+Xc*(l2*cos(foai_12)+l1))/((l2*cos(foai_12)+l1).^2+l2.^2*(sin(foai_12)).^2); %foai_01∈（20,180） foai_01 = acos(cosfoai_01); cosfoai_34 = ((Xc-l0).^2+Yc.^2-l3.^2-l4.^2)/(2*l3*l4); %foai_34∈（pi,2pi） foai_34 = 2*pi - acos(cosfoai_34); A = l0 - Xc; B = l3 * sin(foai_34); C = l4 + l3*cos(foai_34); %foai_t = asin(C/sqrt(B.^2+C.^2)); foai_t = acos(B/sqrt(B.^2+C.^2)); %foai_40∈（0,pi） foai_40 = foai_t - asin(A/sqrt(B.^2+C.^2)); thta1 = foai_01/pi*180; thta2 = 180 - foai_40/pi*180; thta1 = thta1/180*pi; thta2 = thta2/180*pi; endl0代表l5,l1、l2、l3、l4同上，X\Y为C点相对于坐标原点（A点）的位置，返回的参数thta1，thta2为上面φ1，φ4的角度值单位°。 |
【本文地址】
今日新闻 |
推荐新闻 |